#include "move_plan_sys.h"


namespace ljl{

MOVE_PLAN_SYS::MOVE_PLAN_SYS(TRACKPATH* TrackPath, BOOSTPATH* BoostPath, string filename)
	:mTrackPath(TrackPath),
	mBoostPath(BoostPath),
	mposePath(filename)
{
	vector<float> v(180);
	mvspeed.swap(v);
	for(int i=0; i<180; i++){
		mvspeed[i] = mMaxw * float(i)/180.0;
	}

}

/*
	    ^ 90
	    |
	    |
	    |
180 <------------------------> 0


*/
void MOVE_PLAN_SYS::poseCallback(const geometry_msgs::PoseStampedConstPtr& msg)
{
    mpose = *msg;

    double linear_pose[3] = {0};
    double angle_pose[4] = {0,0,1,0};

    angle_pose[0] = mpose.pose.orientation.w;
    angle_pose[1] = mpose.pose.orientation.x;    
    angle_pose[2] = mpose.pose.orientation.y;
    angle_pose[3] = mpose.pose.orientation.z;

    linear_pose[0] = mpose.pose.position.x;
    linear_pose[1] = mpose.pose.position.y;
    linear_pose[2] = mpose.pose.position.z;

    mrobot_x = linear_pose[0];
    mrobot_y = linear_pose[2];

   
    mrobot_w = angle_pose[0];
    miscallback = true;
} 

void MOVE_PLAN_SYS::loadGoalPose(){
	mTrackPath->LoadPath(mBoostPath, mposePath);  //out: mBoostPath->mvTrackPaths
	cout << "loadGoalPose :: size is:  "  << mBoostPath->mvTrackPaths.size() << endl;

        //pPATH->mvTrackPaths.size()

	this->mvTrackPaths = mBoostPath->mvTrackPaths;


}
float MOVE_PLAN_SYS::wad2ang(const float w){
return (w/(2*mPI) ) * 360;
}
#if 1
bool MOVE_PLAN_SYS::xycovert2vw(const float x_start,
					const float y_start,
					const float theta_start,
					const float x_goal,
					const float y_goal,
					const float theta_goal,
					float &vout,
					float &wout
					){
	float x = x_start;
	float y = y_start;
	float theta = theta_start;

	float x_diff = x_goal -x;
	float y_diff = y_goal -y;


	float heading_angle = 0.0;
	float alpha = 0.0;
	float beta = 0.0;
        float v=0.0, w=0.0;

	float dv = 2-0.5;
	float dw = 3-0.5;
	//v:0.5-7()  w: 0.5-3(4.5-28.1)

	float rho = sqrt(x_diff*x_diff + y_diff*y_diff);
	cout << "move_plan_sys.cc :: rho= " << rho << endl; 

	heading_angle = float (atan2(y_diff, x_diff))*(180.0/mPI);   // -180  - 0 -  180

#if 0   
// 0  -  360   shui shi zhen
	if(heading_angle <= 0){
		heading_angle = -heading_angle;
	}
	else{
		heading_angle = 360.0 - heading_angle;
	}
#endif	
	
	if(heading_angle < 0) heading_angle = 360 + heading_angle;	//0 - 360   ni shi zhen		
	float temp = heading_angle - theta;
	if(temp < -m180) {
		alpha = temp + m180*2;
	}
	else if(temp > m180) alpha = temp - m180*2;
	else alpha = temp;

cout <<"move_plan_sys.cc :: heading_angle = " <<  "     " << heading_angle<<endl;
cout << "move_plan_sys.cc :: alpha= heading - theta  " << alpha << endl; 

	temp = theta_goal - heading_angle;
	if(temp < -mPI) beta = temp + 2*m180;
	else if(temp > mPI) beta = temp - 2*m180;
	else beta = temp;

	if(rho < 0.5 && abs(alpha) < 10) {
		cout << "move_plan_sys.cc :: " << "true arrived" <<endl;
		cout << ">>> current x  " << x << endl;
		cout << ">>> current y  " << y << endl;
		cout << ">>> current theta  " << theta << endl;

		cout << ">>> goal x  " << x_goal << endl;
		cout << ">>> goal y  " << y_goal << endl;
		cout << ">>> goal theta  " << theta_goal << endl;
		return true;
	}
#if 0
	float rate_v = 0.3;
	float rate_w = 0.7;
	v = dv * rate_v + 0.5;
	w = dw * rate_v + 0.5;
#endif

    	v = (mKp_rho * rho) * (5/50.0) ;				//distance to v/s

	//beta = beta * (mPI/180.0);
    	//w = (mKp_alpha * alpha + mKp_beta * beta) *(3.0/200.0);		//angle
        //w = (mKp_alpha * alpha*(mPI/180.0)) *(10.0/200.0);
	float abs_alpha = abs(alpha);
	w = (alpha/abs_alpha) * mvspeed[abs_alpha];

cout << ">>> current x  " << x << endl;
cout << ">>> current y  " << y << endl;
cout << ">>> current theta  " << theta << endl;


cout << ">>> goal x  " << x_goal << endl;
cout << ">>> goal y  " << y_goal << endl;
cout << ">>> goal theta  " << theta_goal << endl;


	
	//if(w < mMinw && w > -mMinw) w = mMinw * w/abs(w);
	if(w > mMaxw || w < -mMaxw) w = mMaxw * w/abs(w);
	if(v > mMaxv || v < -mMaxv) v=  mMaxv * v/abs(v);
	
	if(abs_alpha >= 90.0){
		misback = true;	
	}
	if (misback && abs_alpha >  30){
		cout << ">>> back"<<endl;
		v = 0;
		if(abs_alpha > 175){
			w = abs(w);		
		}
	}
	else{misback = false;}

	
	if(rho * sin(alpha*mPI/180.0) < 0.1){
		cout << "distance is " << rho * sin(alpha*mPI/180.0) << endl;
	}
	vout = v ;
	wout = w;
cout << "v = " <<v <<endl;
cout << "w = " << w <<endl;
	mstatus_v = vout;
	mstatus_w = wout;
	return false;

}
#endif

void MOVE_PLAN_SYS::fast_stop(ros::Publisher &pub_cmd, geometry_msgs::Twist &cmd){

    for (int i=0; i<10; i++){
	    cmd.linear.x = 0;
	    cmd.angular.z = 0;
	    pub_cmd.publish(cmd);	//向话题发布消息
	    usleep(1000);
    }

}
void MOVE_PLAN_SYS::slow_stop(ros::Publisher &pub_cmd, geometry_msgs::Twist &cmd){
    float dv = 2;
    float dw = 1;
    float abs_v = abs(mstatus_v);
    float abs_w = abs(mstatus_w);

    while (mstatus_v != 0.0 || mstatus_w != 0.0){
        cout << "slow stop...  "<<endl;

	if(abs_v > dv){
	    mstatus_v = mstatus_v/2;
	}
	else{
cout << "-------------"<<endl;
mstatus_v = 0.0;}
	//
	if(abs_w > dw){
	    mstatus_w /= 2;
	}
	else {mstatus_w = 0.0;}

        cmd.linear.x = mstatus_v;
        cmd.angular.z = mstatus_w;
        pub_cmd.publish(cmd);	//向话题发布消息
        usleep(1000 * 100);
    }

}

void MOVE_PLAN_SYS::slow_start(geometry_msgs::Twist &cmd, float &control_speed, float &control_turn){

	const float dv = 0.2;
	const float dw = 0.1;
	
	const float goal_v = cmd.linear.x;
	const float goal_w = cmd.angular.z;
	if (goal_v > control_speed){
		control_speed = min(goal_v, control_speed + dv);
	}
	else if(goal_v < control_speed){
		control_speed = max(goal_v, control_speed - dv);
	}
	else{
		control_speed = goal_v;
	}

	//turn 
	if(goal_w > control_turn){
		control_speed = min(goal_w, control_turn + dw);
	}
	else if(cmd.angular.z < control_turn){
		control_speed = max(goal_w, control_turn - dw);
	}
	else{
		control_turn = goal_w;
	}
	cmd.linear.x = goal_v;
	cmd.angular.z = goal_w;

}



//end
}
